#include <iostream>
#include <Eigen/Dense>

static bool LeastSquareMethod(Eigen::VectorXf& x, Eigen::VectorXf& y, uint8_t orders, Eigen::VectorXf& coeffs)
{
    Eigen::MatrixXf mtxVandermonde(x.size(), orders + 1);   // Vandermonde matrix of X-axis coordinate vector of sample data
    Eigen::VectorXf vectColVandermonde = x;                 // Vandermonde column
    Eigen::VectorXf vectResult;

    if ((x.size() < 2) || (y.size() < 2) || (x.size() != y.size()) || (orders < 1))
    {
        return false;
    }

    mtxVandermonde.col(0) = Eigen::VectorXf::Constant(x.size(), 1, 1);
    mtxVandermonde.col(1) = vectColVandermonde;

    // construct Vandermonde matrix column by column
    for (int32_t i = 2; i < orders + 1; i++)
    {
        vectColVandermonde = vectColVandermonde.array() * x.array();
        mtxVandermonde.col(i) = vectColVandermonde;
    }

    // calculate coefficients vector of fitted polynomial
    coeffs = (mtxVandermonde.transpose() * mtxVandermonde).inverse() * mtxVandermonde.transpose() * y;

    return true;
}

int test_main2()
{
    Eigen::VectorXf x(5);
    Eigen::VectorXf y(5);

    x(0) = 1;
    x(1) = 2;
    x(2) = 3;
    x(3) = 4;
    x(4) = 5;

    y(0) = 3;
    y(1) = 5;
    y(2) = 8;
    y(3) = 9;
    y(4) = 12;

    Eigen::VectorXf coeffs;
    //LeastSquareMethod(x, y, 3, coeffs);
    LeastSquareMethod(x, y, 2, coeffs);

    std::cout << "The coefficients vector is: \n" << std::endl;
    for (int32_t i = 0; i < coeffs.size(); i++)
    {
        std::cout << "coeffs" << i << ": " << coeffs(i) << std::endl;
    }

    // 残差计算
    Eigen::VectorXf y_pred = coeffs(0) + coeffs(1) * x.array() + coeffs(2) * x.array().square();
    std::cout << "残差平方和: " << (y - y_pred).squaredNorm() << std::endl;
    return 0;
}